//头文件
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "sentry_interfaces/msg/robot_hp.hpp"

#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/bt_factory.h"
#include <iostream>
namespace mynode
{
    using BT::NodeStatus;
    //行为树节点类

    //oriented_point叶节点
    class oriented_point : public BT::SyncActionNode
    {
        public:
            oriented_point(const std::string& name, const BT::NodeConfig& config) :
            BT::SyncActionNode(name, config)
            {
            }
            // 给该节点申明端口
            static BT::PortsList providedPorts()
            {
                return { 
                    //端口设置
                        BT::OutputPort<std::string>("start_op"),
                        BT::InputPort<std::string>("x"),
                        BT::InputPort<std::string>("y"),
                };
            }
            BT::NodeStatus tick() override
            {
                //实现功能
                std::string x;
                std::string y;
                getInput("x",x);
                getInput("y",y);
                setOutput("start_op",x+';'+y);
                //getInput("端口名称",端口输入值);
                return BT::NodeStatus::SUCCESS;
            }
    };



    //radar_robot_dect叶节点
    class radar_robot_dect : public BT::SyncActionNode
    {
        public:
            radar_robot_dect(const std::string& name, const BT::NodeConfig& config) :
            BT::SyncActionNode(name, config)
            {
            }
            // 给该节点申明端口
            static BT::PortsList providedPorts()
            {
                return { 
                    //端口设置
                        BT::OutputPort<geometry_msgs::msg::Twist>("cmd_gimbal"),
                        BT::InputPort<std_msgs::msg::Float32MultiArray>("robot_theta"),
                };
            }
            BT::NodeStatus tick() override
            {
                geometry_msgs::msg::Twist cmd_gimbal_value_;
                std_msgs::msg::Float32MultiArray robot_theta_value_; 
                //实现功能
                getInput("robot_theta",robot_theta_value_);
                cmd_gimbal_value_=geometry_msgs::msg::Twist();
                cmd_gimbal_value_.angular.z=(double)robot_theta_value_.data[0]/3.14*180+180;
                setOutput("cmd_gimbal",cmd_gimbal_value_);
                return BT::NodeStatus::SUCCESS;
            }
    };



    //hp_range叶节点
    class hp_range : public BT::SyncActionNode
    {
        public:
            hp_range(const std::string& name, const BT::NodeConfig& config) :
            BT::SyncActionNode(name, config)
            {
            }
            // 给该节点申明端口
            static BT::PortsList providedPorts()
            {
                return { 
                    //端口设置
                        BT::InputPort<sentry_interfaces::msg::RobotHP>("robot_hp"),
                        BT::InputPort<std::string>("friend_or_foe"),
                        BT::InputPort<std::string>("robot_id"),
                        BT::InputPort<std::string>("hp_min"),
                        BT::InputPort<std::string>("hp_max"),
                };
            }
            BT::NodeStatus tick() override
            {
                //功能实现
                int hp_goal;
                int hp_max_value;
                int hp_min_value;
                int is_friend_value;
                int robot_id_value;
                sentry_interfaces::msg::RobotHP robot_hp_value;

                getInput("hp_max",hp_max_value);
                getInput("hp_min",hp_min_value);
                getInput("friend_or_foe",is_friend_value);
                getInput("robot_id",robot_id_value);
                getInput("robot_hp",robot_hp_value);

                if(is_friend_value==0)
                    hp_goal=robot_hp_value.mate_hp[robot_id_value];
                else
                    hp_goal=robot_hp_value.enermy_hp[robot_id_value];
                
                if(hp_goal>hp_min_value && hp_goal<hp_max_value)
                    return BT::NodeStatus::SUCCESS;
                else
                    return BT::NodeStatus::FAILURE;
            }
    };

    //goal_pose_set叶节点
    class goal_pose_set : public BT::SyncActionNode
    {
        public:
            goal_pose_set(const std::string& name, const BT::NodeConfig& config) :
            BT::SyncActionNode(name, config)
            {
            }
            // 给该节点申明端口
            static BT::PortsList providedPorts()
            {
                return { 
                    //端口设置
                        BT::OutputPort<std::string>("goal_pose_bt"),
                        BT::InputPort<std::string>("goal_y"),
                        BT::InputPort<std::string>("goal_x"),
                };
            }
            BT::NodeStatus tick() override
            {
                //实现功能
                std::string goal_x_="0.0";
                std::string goal_y_="0.0";
                getInput("goal_x",goal_x_);
                getInput("goal_y",goal_y_);
                setOutput("goal_pose_bt",goal_x_+";"+goal_y_);
                
                return BT::NodeStatus::SUCCESS;
            }
    };


    
    inline void RegisterNodes(BT::BehaviorTreeFactory& factory)
    {
        //注册节点
factory.registerNodeType<oriented_point>("oriented_point");

factory.registerNodeType<radar_robot_dect>("radar_robot_dect");

factory.registerNodeType<hp_range>("hp_range");

factory.registerNodeType<goal_pose_set>("goal_pose_set");


    }
}